#include "servo.h"

TIM_HandleTypeDef* servo_htim;
uint32_t servo_channel;
int32_t servo_range_min = 0;
int32_t servo_range_max = 0;
int32_t servo_range = 0;

/**
  * @brief  Init servo 
  * @param	handle of tim, channels, min range, max range
  * @retval None
  */
void servo_init(TIM_HandleTypeDef* htim, uint32_t channel, int32_t min, int32_t max)
{
	servo_htim = htim;
	servo_channel = channel;
	servo_range_min = min + 1;
	servo_range_max = max + 1;
	servo_range = servo_range_max - servo_range_min;
	float compare = (servo_range_max + servo_range_min) / 2;
	__HAL_TIM_SetCompare(servo_htim, servo_channel, compare);
}

static float servo_limit(float degree)
{
	float result = 0;
	result = (degree > 1) ? 1 : degree;
	result = (degree <	-1) ? -1 : degree;
	return result;
}

/**
  * @brief  set servo angle 
  * @param	degree [-1, 1]
  * @retval None
  */
void servo_set(float degree)
{
	degree = servo_limit(degree);
	float compare = (servo_range_min + servo_range_max) / 2 + degree * servo_range / 2;
	__HAL_TIM_SetCompare(servo_htim, servo_channel, compare);
}